import serial
import matplotlib.pyplot as plt
import numpy as np
import time

# --- Serial setup ---
ser = serial.Serial('COM4', 115200, timeout=1)  # <-- Change COM port if needed
time.sleep(2)  # wait for ESP32 to reset

# --- Matplotlib setup ---
plt.ion()  # interactive mode
fig, ax = plt.subplots(subplot_kw={'projection': 'polar'})
ax.set_theta_zero_location("N")
ax.set_theta_direction(-1)
ax.set_ylim(0, 100)  # max distance in cm

points, = ax.plot([], [], 'go')  # green dots
angles = []
distances = []

while True:
    try:
        line = ser.readline().decode().strip()
        if not line:
            plt.pause(0.01)  # let window update
            continue

        parts = line.split(',')
        if len(parts) != 2:
            continue

        angle_str, distance_str = parts
        try:
            angle = int(angle_str)
            distance = float(distance_str)
        except:
            continue  # skip invalid values

        # Convert to polar coords
        theta = np.deg2rad(angle)
        r = distance

        # Store latest point
        angles.append(theta)
        distances.append(r)

        # Keep trail short (last 200 points)
        if len(angles) > 200:
            angles = angles[-200:]
            distances = distances[-200:]

        # Clear and redraw
        ax.clear()
        ax.set_theta_zero_location("N")
        ax.set_theta_direction(-1)
        ax.set_ylim(0, 100)
        ax.scatter(angles, distances, c='red', s=30)  # trail of points

        plt.pause(0.01)  # keeps window responsive

    except KeyboardInterrupt:
        print("Exiting...")
        break

ser.close()

